home *** CD-ROM | disk | FTP | other *** search
/ TOS Silver 2000 / TOS Silver 2000.iso / Anwendun / Pov / POVTOOLS / FTPOVRAY / 3D2POV18 / SOURCE / VECT.C < prev    next >
Encoding:
C/C++ Source or Header  |  1995-11-25  |  8.9 KB  |  403 lines
  1. #define __GNUC__
  2.  
  3. #include <portab.h>
  4. #include <math.h>
  5. #include <string.h>
  6. #include "vect.h"
  7.  
  8. #define EPSILON 1e-6
  9.  
  10. void vect_init (Vector v, float  x, float  y, float  z)
  11. {
  12.     v[X] = x;
  13.     v[Y] = y;
  14.     v[Z] = z;
  15. }
  16.  
  17.  
  18. void vect_copy (Vector v1, Vector v2)
  19. {
  20.     v1[X] = v2[X];
  21.     v1[Y] = v2[Y];
  22.     v1[Z] = v2[Z];
  23. }
  24.  
  25.  
  26. int vect_equal (Vector v1, Vector v2)
  27. {
  28.     if (v1[X] == v2[X] && v1[Y] == v2[Y] && v1[Z] == v2[Z])
  29.     return 1;
  30.     else
  31.     return 0;
  32. }
  33.  
  34.  
  35. void vect_add (Vector v1, Vector v2, Vector v3)
  36. {
  37.     v1[X] = v2[X] + v3[X];
  38.     v1[Y] = v2[Y] + v3[Y];
  39.     v1[Z] = v2[Z] + v3[Z];
  40. }
  41.  
  42.  
  43. void vect_sub (Vector v1, Vector v2, Vector v3)
  44. {
  45.     v1[X] = v2[X] - v3[X];
  46.     v1[Y] = v2[Y] - v3[Y];
  47.     v1[Z] = v2[Z] - v3[Z];
  48. }
  49.  
  50.  
  51. void vect_scale (Vector v1, Vector v2, float  k)
  52. {
  53.     v1[X] = k * v2[X];
  54.     v1[Y] = k * v2[Y];
  55.     v1[Z] = k * v2[Z];
  56. }
  57.  
  58.  
  59. float vect_mag (Vector v)
  60. {
  61.     float mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
  62.  
  63.     return mag;
  64. }
  65.  
  66.  
  67. void vect_normalize (Vector v)
  68. {
  69.     float mag = vect_mag (v);
  70.  
  71.     if (mag > 0.0)
  72.     vect_scale (v, v, 1.0/mag);
  73. }
  74.  
  75.  
  76. float vect_dot (Vector v1, Vector v2)
  77. {
  78.     return (v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z]);
  79. }
  80.  
  81.  
  82. void vect_cross (Vector v1, Vector v2, Vector v3)
  83. {
  84.     v1[X] = (v2[Y] * v3[Z]) - (v2[Z] * v3[Y]);
  85.     v1[Y] = (v2[Z] * v3[X]) - (v2[X] * v3[Z]);
  86.     v1[Z] = (v2[X] * v3[Y]) - (v2[Y] * v3[X]);
  87. }
  88.  
  89. void vect_min (Vector v1, Vector v2, Vector v3)
  90. {
  91.     v1[X] = (v2[X] < v3[X]) ? v2[X] : v3[X];
  92.     v1[Y] = (v2[Y] < v3[Y]) ? v2[Y] : v3[Y];
  93.     v1[Z] = (v2[Z] < v3[Z]) ? v2[Z] : v3[Z];
  94. }
  95.  
  96.  
  97. void vect_max (Vector v1, Vector v2, Vector v3)
  98. {
  99.     v1[X] = (v2[X] > v3[X]) ? v2[X] : v3[X];
  100.     v1[Y] = (v2[Y] > v3[Y]) ? v2[Y] : v3[Y];
  101.     v1[Z] = (v2[Z] > v3[Z]) ? v2[Z] : v3[Z];
  102. }
  103.  
  104.  
  105. /* Return the angle between two vectors */
  106. float vect_angle (Vector v1, Vector v2)
  107. {
  108.     float  mag1, mag2, angle, cos_theta;
  109.  
  110.     mag1 = vect_mag(v1);
  111.     mag2 = vect_mag(v2);
  112.  
  113.     if (mag1 * mag2 == 0.0)
  114.     angle = 0.0;
  115.     else {
  116.     cos_theta = vect_dot(v1,v2) / (mag1 * mag2);
  117.  
  118.     if (cos_theta <= -1.0)
  119.         angle = 180.0;
  120.     else if (cos_theta >= +1.0)
  121.         angle = 0.0;
  122.     else
  123.         angle = (180.0/M_PI) * acos(cos_theta);
  124.     }
  125.  
  126.     return angle;
  127. }
  128.  
  129.  
  130. void vect_print (FILE *f, Vector v, int dec, char sep)
  131. {
  132.     char fstr[] = "%.4f, %.4f, %.4f";
  133.  
  134.     if (dec < 0) dec = 0;
  135.     if (dec > 9) dec = 9;
  136.  
  137.     fstr[2]  = '0' + dec;
  138.     fstr[8]  = '0' + dec;
  139.     fstr[14] = '0' + dec;
  140.  
  141.     fstr[4]  = sep;
  142.     fstr[10] = sep;
  143.  
  144.     fprintf (f, fstr, v[X], v[Y], v[Z]);
  145. }
  146.  
  147.  
  148. /* Rotate a vector about the X, Y or Z axis */
  149. void vect_rotate (Vector v1, Vector v2, int axis, float angle)
  150. {
  151.     float  cosa, sina;
  152.  
  153.     cosa = cos ((M_PI/180.0) * angle);
  154.     sina = sin ((M_PI/180.0) * angle);
  155.  
  156.     switch (axis) {
  157.     case X:
  158.         v1[X] =  v2[X];
  159.         v1[Y] =  v2[Y] * cosa + v2[Z] * sina;
  160.         v1[Z] =  v2[Z] * cosa - v2[Y] * sina;
  161.         break;
  162.  
  163.     case Y:
  164.         v1[X] = v2[X] * cosa - v2[Z] * sina;
  165.         v1[Y] = v2[Y];
  166.         v1[Z] = v2[Z] * cosa + v2[X] * sina;
  167.         break;
  168.  
  169.     case Z:
  170.         v1[X] = v2[X] * cosa + v2[Y] * sina;
  171.         v1[Y] = v2[Y] * cosa - v2[X] * sina;
  172.         v1[Z] = v2[Z];
  173.         break;
  174.     }
  175. }
  176.  
  177.  
  178. /* Rotate a vector about a specific axis */
  179. void vect_axis_rotate (Vector v1, Vector v2, Vector axis, float angle)
  180. {
  181.     float  cosa, sina;
  182.     Matrix mx;
  183.  
  184.     cosa = cos ((M_PI/180.0) * angle);
  185.     sina = sin ((M_PI/180.0) * angle);
  186.  
  187.     mx[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
  188.     mx[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
  189.     mx[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
  190.     mx[0][3] = 0.0;
  191.  
  192.     mx[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
  193.     mx[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
  194.     mx[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
  195.     mx[1][3] = 0.0;
  196.  
  197.     mx[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
  198.     mx[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
  199.     mx[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
  200.     mx[2][3] = 0.0;
  201.  
  202.     mx[3][0] = mx[3][1] = mx[3][2] = mx[3][3] = 0.0;
  203.  
  204.     vect_transform (v1, v2, mx);
  205. }
  206.  
  207.  
  208. /* Create an identity matrix */
  209. void mx_identity (Matrix mx)
  210. {
  211.     int i, j;
  212.  
  213.     for (i = 0; i < 4; i++)
  214.     for (j = 0; j < 4; j++)
  215.         mx[i][j] = 0.0;
  216.  
  217.     for (i = 0; i < 4; i++)
  218.     mx[i][i] = 1.0;
  219. }
  220.  
  221.  
  222. /* Rotate a matrix about the X, Y or Z axis */
  223. void mx_rotate (Matrix mx1, Matrix mx2, int axis, float angle)
  224. {
  225.     Matrix mx;
  226.     float  cosa, sina;
  227.  
  228.     cosa = cos ((M_PI/180.0) * angle);
  229.     sina = sin ((M_PI/180.0) * angle);
  230.  
  231.     mx_identity (mx);
  232.  
  233.     switch (axis) {
  234.     case X:
  235.         mx[1][1] = cosa;
  236.         mx[1][2] = sina;
  237.         mx[2][1] = -sina;
  238.         mx[2][2] = cosa;
  239.         break;
  240.  
  241.     case Y:
  242.         mx[0][0] = cosa;
  243.         mx[0][2] = -sina;
  244.         mx[2][0] = sina;
  245.         mx[2][2] = cosa;
  246.         break;
  247.  
  248.     case Z:
  249.         mx[0][0] = cosa;
  250.         mx[0][1] = sina;
  251.         mx[1][0] = -sina;
  252.         mx[1][1] = cosa;
  253.         break;
  254.     }
  255.  
  256.     mx_mult (mx1, mx2, mx);
  257. }
  258.  
  259.  
  260. void mx_axis_rotate (Matrix mx1, Matrix mx2, Vector axis, float angle)
  261. {
  262.     float  cosa, sina;
  263.     Matrix mx;
  264.  
  265.     cosa = cos ((M_PI/180.0) * angle);
  266.     sina = sin ((M_PI/180.0) * angle);
  267.  
  268.     mx[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
  269.     mx[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
  270.     mx[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
  271.     mx[0][3] = 0.0;
  272.  
  273.     mx[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
  274.     mx[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
  275.     mx[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
  276.     mx[1][3] = 0.0;
  277.  
  278.     mx[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
  279.     mx[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
  280.     mx[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
  281.     mx[2][3] = 0.0;
  282.  
  283.     mx[3][0] = mx[3][1] = mx[3][2] = mx[3][3] = 0.0;
  284.  
  285.     mx_mult (mx1, mx2, mx);
  286. }
  287.  
  288.  
  289. void mx_mult (Matrix mx1, Matrix mx2, Matrix mx3)
  290. {
  291.     float sum;
  292.     int   i, j, k;
  293.  
  294.     for (i = 0; i < 4; i++) {
  295.     for (j = 0; j < 4; j++) {
  296.         sum = 0.0;
  297.  
  298.         for (k = 0; k < 4; k++)
  299.         sum = sum + mx2[i][k] * mx3[k][j];
  300.  
  301.         mx1[i][j] = sum;
  302.     }
  303.     }
  304. }
  305.  
  306.  
  307. /* Transform the given vector */
  308. void vect_transform (Vector v1, Vector v2, Matrix mx)
  309. {
  310.     Vector tmp;
  311.  
  312.     tmp[X] = (v2[X] * mx[0][0]) + (v2[Y] * mx[1][0]) + (v2[Z] * mx[2][0]) + mx[3][0];
  313.     tmp[Y] = (v2[X] * mx[0][1]) + (v2[Y] * mx[1][1]) + (v2[Z] * mx[2][1]) + mx[3][1];
  314.     tmp[Z] = (v2[X] * mx[0][2]) + (v2[Y] * mx[1][2]) + (v2[Z] * mx[2][2]) + mx[3][2];
  315.  
  316.     vect_copy (v1, tmp);
  317. }
  318.  
  319.  
  320.  
  321. /*
  322.    Decodes a 3x4 transformation matrix into separate scale, rotation,
  323.    translation, and shear vectors. Based on a program by Spencer W.
  324.    Thomas (Graphics Gems II)
  325. */
  326. void mx_decode (Matrix mx, Vector scale,  Vector shear, Vector rotate,
  327.         Vector transl)
  328. {
  329.     int i;
  330.     Vector row[3], temp;
  331.  
  332.     for (i = 0; i < 3; i++)
  333.     transl[i] = mx[3][i];
  334.  
  335.     for (i = 0; i < 3; i++) {
  336.     row[i][X] = mx[i][0];
  337.     row[i][Y] = mx[i][1];
  338.     row[i][Z] = mx[i][2];
  339.     }
  340.  
  341.     scale[X] = vect_mag (row[0]);
  342.     vect_normalize (row[0]);
  343.  
  344.     shear[X] = vect_dot (row[0], row[1]);
  345.     row[1][X] = row[1][X] - shear[X]*row[0][X];
  346.     row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
  347.     row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
  348.  
  349.     scale[Y] = vect_mag (row[1]);
  350.     vect_normalize (row[1]);
  351.  
  352.     if (scale[Y] != 0.0)
  353.     shear[X] /= scale[Y];
  354.  
  355.     shear[Y] = vect_dot (row[0], row[2]);
  356.     row[2][X] = row[2][X] - shear[Y]*row[0][X];
  357.     row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
  358.     row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
  359.  
  360.     shear[Z] = vect_dot (row[1], row[2]);
  361.     row[2][X] = row[2][X] - shear[Z]*row[1][X];
  362.     row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
  363.     row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
  364.  
  365.     scale[Z] = vect_mag (row[2]);
  366.     vect_normalize (row[2]);
  367.  
  368.     if (scale[Z] != 0.0) {
  369.     shear[Y] /= scale[Z];
  370.     shear[Z] /= scale[Z];
  371.     }
  372.  
  373.     vect_cross (temp, row[1], row[2]);
  374.     if (vect_dot (row[0], temp) < 0.0) {
  375.     for (i = 0; i < 3; i++) {
  376.         scale[i]  *= -1.0;
  377.         row[i][X] *= -1.0;
  378.         row[i][Y] *= -1.0;
  379.         row[i][Z] *= -1.0;
  380.     }
  381.     }
  382.  
  383.     if (row[0][Z] < -1.0) row[0][Z] = -1.0;
  384.     if (row[0][Z] > +1.0) row[0][Z] = +1.0;
  385.  
  386.     rotate[Y] = asin(-row[0][Z]);
  387.  
  388.     if (fabs(cos(rotate[Y])) > EPSILON) {
  389.     rotate[X] = atan2 (row[1][Z], row[2][Z]);
  390.     rotate[Z] = atan2 (row[0][Y], row[0][X]);
  391.     }
  392.     else {
  393.     rotate[X] = atan2 (row[1][X], row[1][Y]);
  394.     rotate[Z] = 0.0;
  395.     }
  396.  
  397.     /* Convert the rotations to degrees */
  398.     rotate[X] = (180.0/M_PI)*rotate[X];
  399.     rotate[Y] = (180.0/M_PI)*rotate[Y];
  400.     rotate[Z] = (180.0/M_PI)*rotate[Z];
  401. }
  402.  
  403.